
#pragma once

#include <optional>
#include <string>
#include <sstream>

#include <behaviortree_cpp/behavior_tree.h>
#include <tl_expected/expected.hpp>

namespace fishbot::behavior_tree
{

    /**
     * @brief Filter function for testing if a result has an error
     *
     * @param exp The expected type to test
     * @tparam T The value type
     * @tparam E The error type
     * @return if the expected has an error
     */
    template <typename T, typename E>
    constexpr bool has_error(const tl::expected<T, E> &exp)
    {
        return !exp;
    }
    template <typename T>
    constexpr bool has_error(const BT::Expected<T> &exp)
    {
        return !exp;
    }

    // Implementation for BT::Expected
    template <typename... Args>
    std::optional<std::string> maybe_error(BT::Expected<Args>... args)
    {
        std::ostringstream errorStream;
        bool hasError = false;

        // Check each BT::Expected for error
        (( has_error(args)? (errorStream << args.error() << " ", hasError = true) : 0), ...);

        return hasError ? std::make_optional<std::string>(errorStream.str()) : std::nullopt;
    }

    // Implementation for tl::expected
    template <typename E, typename... Args>
    constexpr std::optional<E> maybe_error(tl::expected<Args, E>... args)
    {
        constexpr size_t size = sizeof...(Args);
        E errorMessages[size];

        // Collect error messages from tl::expected
        size_t index = 0;
        ((has_error(args) ? (errorMessages[index++] = args.error()) : 0), ...);

        // Return the first error if any, else nothing
        return index > 0 ? std::make_optional<E>(errorMessages[0]) : std::nullopt;
    }

} // namespace moveit_studio::behaviors